/*
 * @Description: Bezier曲线定义
 * @Author: Dryad
 * @Date: 2019-08-26 14:27:19
 * @LastEditors: Dryad
 * @LastEditTime: 2019-09-17 10:20:07
 * @Encoding: utf-8
 */

class Bezier_Curve_Class
{
public:
    union Bezier_Control_Coor {
        float coor[3]; /* Bezier曲线控制点的3轴坐标 */
        struct
        {
            float x; /* 控制点x轴坐标 */
            float y; /* 控制点y轴坐标 */
            float z; /* 控制点z轴坐标 */
        };
    };
    const union Bezier_Control_Coor *P1; /* Bezier曲线第一个控制点 */
    union Bezier_Control_Coor P4;        /* Bezier曲线第四个控制点 */

    Bezier_Curve_Class *pre_curve, *next_curve;   /* 指向前一条Bezier曲线和指向后一条Bezier曲线 */
    static float curve_u;                         /* 曲线参数u */
    static float curve_length;                    /* 参数u对应的曲线长度 */
    static union Bezier_Control_Coor target_coor; /* 目标坐标 */
    static union Bezier_Control_Coor target_v;    /* 目标速度方向 */
    float sum_length;                             /* 曲线总长 */

    float Generate_Curve(const union Bezier_Control_Coor *const _P1); /* 根据输入起点生成Bezier曲线，返回曲线长度 */
    bool Cal_Target(const union Bezier_Control_Coor &current_coor);   /* 根据当前坐标计算目标坐标和目标速度方向,返回false表示该段曲线运行完毕 */

private:
    union Bezier_Control_Coor P2, P3; /* Bezier曲线中间两个控制点，为了节省SRAM空间和降低不必要的运算，此处的P2和P3已放大3倍 */

    float Cal_Length(const float curve_u); /* 根据输入曲线参数计算对应的曲线长度 */

    struct integration_range_struct
    {
        /* 三阶Bezier曲线导数为2次，单调区间解有2个 */
        struct
        {
            float interval; /* 积分区间 */
            float value;    /* 积分区间对应的值 */
        } minor, larger;    /* 较小解，较大解 */
        float left_value;   /* 左值 */
        float right_value;  /* 右值 */
    };
    struct integration_range_struct range[3]; /* 三个区间，对应x,y,z */

    struct interval_length_struct
    {
        float left_length;   /* 左区间长度 */
        float middle_length; /* 中间区间长度 */
        float right_length;  /* 右边区间长度 */
    };
    struct interval_length_struct interval_length[3]; /* 三个区间，对应x,y,z */

    void Cal_Range(float a, float b, float c, struct integration_range_struct &range); /* 根据abc的值计算单调区间，abc为二次方程的系数 */

    union Bezier_Control_Coor Func_Bezier(const float u);      /* 计算Bezier曲线对应的值，此处即为坐标 */
    union Bezier_Control_Coor ka, kb, kc;                      /* Bezier曲线一阶导的3个系数 */
    union Bezier_Control_Coor Func_Bezier_Diff(const float u); /* 计算Bezier曲线一阶导数对应的值，此处即为速度方向 */

    static float Cal_1Norm(const union Bezier_Curve_Class::Bezier_Control_Coor &A); /* 计算1范数 */
    static float Cal_2Norm(const union Bezier_Curve_Class::Bezier_Control_Coor &A); /* 计算2范数 */
};

union Bezier_Curve_Class::Bezier_Control_Coor operator+(const union Bezier_Curve_Class::Bezier_Control_Coor &A,
                                                        const union Bezier_Curve_Class::Bezier_Control_Coor &B);
union Bezier_Curve_Class::Bezier_Control_Coor operator-(const union Bezier_Curve_Class::Bezier_Control_Coor &A,
                                                        const union Bezier_Curve_Class::Bezier_Control_Coor &B);
union Bezier_Curve_Class::Bezier_Control_Coor operator*(const union Bezier_Curve_Class::Bezier_Control_Coor &A,
                                                        const float factor);
